//
// File: desired_input_v2.cpp
//
// MATLAB Coder version            : 5.4
// C/C++ source code generated on  : 28-Dec-2024 22:05:47
//

// Include Files
#include "desired_input_v2.h"
#include <cmath>

// Function Definitions
//
// Arguments    : double Xd
//                double Yd
//                double car_angle_d
//                double tarX
//                double tarY
//                double xo
//                double yo
//                double *dgd
//                double *dobd
//                double *sigd
//                double *siod
// Return Type  : void
//
void desired_input_v2(double Xd, double Yd, double car_angle_d, double tarX,
                      double tarY, double xo, double yo, double *dgd,
                      double *dobd, double *sigd, double *siod)
{
  double absxk;
  double dgd_ve_idx_0;
  double dgd_ve_idx_1;
  double scale;
  double t;
  // dgd
  dgd_ve_idx_0 = tarX - Xd;
  dgd_ve_idx_1 = tarY - Yd;
  scale = 3.3121686421112381E-170;
  // sigd
  absxk = std::abs(dgd_ve_idx_0);
  if (absxk > 3.3121686421112381E-170) {
    *dgd = 1.0;
    scale = absxk;
  } else {
    t = absxk / 3.3121686421112381E-170;
    *dgd = t * t;
  }
  absxk = std::abs(dgd_ve_idx_1);
  if (absxk > scale) {
    t = scale / absxk;
    *dgd = *dgd * t * t + 1.0;
    scale = absxk;
  } else {
    t = absxk / scale;
    *dgd += t * t;
  }
  *dgd = scale * std::sqrt(*dgd);
  *sigd = std::acos(dgd_ve_idx_0 / *dgd);
  if (dgd_ve_idx_1 < 0.0) {
    *sigd = -*sigd - car_angle_d;
  } else {
    *sigd -= car_angle_d;
  }
  // dobd
  dgd_ve_idx_0 = xo - Xd;
  dgd_ve_idx_1 = yo - Yd;
  scale = 3.3121686421112381E-170;
  // siod
  absxk = std::abs(dgd_ve_idx_0);
  if (absxk > 3.3121686421112381E-170) {
    *dobd = 1.0;
    scale = absxk;
  } else {
    t = absxk / 3.3121686421112381E-170;
    *dobd = t * t;
  }
  absxk = std::abs(dgd_ve_idx_1);
  if (absxk > scale) {
    t = scale / absxk;
    *dobd = *dobd * t * t + 1.0;
    scale = absxk;
  } else {
    t = absxk / scale;
    *dobd += t * t;
  }
  *dobd = scale * std::sqrt(*dobd);
  if (dgd_ve_idx_1 < 0.0) {
    *siod = -std::acos(dgd_ve_idx_0 / *dobd) - car_angle_d;
  } else {
    *siod = *sigd - car_angle_d;
  }
}

//
// File trailer for desired_input_v2.cpp
//
// [EOF]
//
